﻿// Inner Fire 游戏引擎库
// PhysicsWorld - 物理世界
//
// Copyright (c) 2025 by 尔西大帝. All rights reserved.
//
// 所属层级：应用层（4）
// 用于统一管理世界。我在想，我是不是要一个这样多余的东西。是否可以将其合并到Objects的管理中？
//
// Author: 尔西大帝
// Email: 2670613871@qq.com
// Created: 2025-06-17

#include "physics_world.h"
#include <game/object/game_object.h>
#include <game/physics/collision.h>
#include <game/physics/constraint.h>
#include <game/physics/rigid_body.h>
#include <klog.h>

namespace ifire::game {

PhysicsWorld::PhysicsWorld(Scene& scene, ikit::sstr _name)
    : scene_(scene)
    , name(_name)
    , use_collision_(true)
    , use_box_(false)
    , use_peneration_constration_(true) {}

PhysicsWorld::~PhysicsWorld() {
  for (auto i : constraints_) {
    delete i;
  }
}

void PhysicsWorld::FixedUpdate(float delta_time) {
  // 进行更新
  for (auto a : objects_) {
    // 如果是静态项目，不更新
    if (a->IsStatic()) {
      continue;
    }

    // 设置重力
    a->AddGravity(gravity_);
    // 设置摩擦力
    if (friction_ > F_EPSILON) {
      a->AddFriction(friction_);
    }
    // 设置阻力
    if (drag_ > F_EPSILON) {
      a->AddDrag(drag_);
    }

    // 为项目增加力
    for (auto& f : forces_) {
      a->AddForce(f);
    }
    for (auto& t : torques_) {
      a->AddTorque(t);
    }
  }

  // 先对力进行计算
  for (auto& body : objects_) {
    body->IntegrateForces(delta_time);
  }

  // 处理碰撞
  ExecuteCollision();

  // 处理约束
  SolveConstraints(delta_time);

  // 对速度进行限制
  for (auto& body : objects_) {
    body->ClampVelocities();
    // 能量监控
    body->MonitorEnergy();
  }

  // 对速度进行积分
  for (auto& body : objects_) {
    body->IntegrateVelocities(delta_time);
  }
}

void PhysicsWorld::LateUpdate(float delta_time) {

  // 对对象进行更新
  for (auto i : objects_) {
    // 使用Box限制
    if (use_box_) {
      KeepInBox(i);
    }

    i->LateUpdate(delta_time);
  }
}

void PhysicsWorld::ExecuteCollision() {
  if (!use_collision_) return;

  for (int i = 0; i < objects_.size() - 1; i++) {
    RigidBody* a = objects_[i];

    for (int j = i + 1; j < objects_.size(); j++) {
      RigidBody* b = objects_[j];
      // 跳过静态物体之间的碰撞
      if (a->IsStatic() && b->IsStatic()) continue;

      // 检查是否碰撞
      Contact c;
      if (use_peneration_constration_) {

        // 如果A是静态物体，则使用B为主碰撞
        if (a->IsStatic()) {
          if (Collision::IsColliding(b, a, c)) {
            c.ResolvePenetration(0.33f);
            penetrations_.emplace_back(b, a, c.start, c.end, c.normal);
          }
        } else {
          if (Collision::IsColliding(a, b, c)) {
            c.ResolvePenetration(0.33f);
            penetrations_.emplace_back(a, b, c.start, c.end, c.normal);
          }
        }
      } else {
        if (Collision::IsColliding(a, b, c)) {
          c.ResolveImpulseRotation();
          //c.ResolvePenetration(1.0f);
        }
      }
    }
  }
}

void PhysicsWorld::SolveConstraints(float dt) {
  for (auto& constraint : penetrations_) {
    constraint.PreSolve(dt);
  }
  for (auto& constraint : constraints_) {
    constraint->PreSolve(dt);
  }
  // TIPS：一个非常奇怪的现象。我这里的迭代步数为1的时候，非常正常（只是回弹的速度慢）
  // 但如果大于1，就会出现抖动的现象。Why？
  for (int i = 0; i < 3; i++) {
    for (auto& constraint : penetrations_) {
      constraint.Solve();
    }
    for (auto& constraint : constraints_) {
      constraint->Solve();
    }
  }

  for (auto& constraint : penetrations_) {
    constraint.PostSolve();
  }
  for (auto& constraint : constraints_) {
    constraint->PostSolve();
  }

  penetrations_.clear();
}

void PhysicsWorld::SetGravity(IVec3 value) { gravity_ = value; }

void PhysicsWorld::SetDrag(float value) { drag_ = value; }

void PhysicsWorld::SetFriction(float value) { friction_ = value; }

void PhysicsWorld::Add(RigidBody* rigid_body) {
  objects_.push_back(rigid_body);
}

void PhysicsWorld::AddJoint(RigidBody* a, RigidBody* b, IVec3 anchor) {
  auto constraint = new JointConstraint(a, b, anchor);
  AddConstraint(constraint);
}

void PhysicsWorld::AddConstraint(Constraint* constraint) {
  constraints_.push_back(constraint);
}

// 加入力
void PhysicsWorld::AddForce(IVec3 value) { forces_.push_back(value); }

// 加入扭矩
void PhysicsWorld::AddTorque(IVec3 value) { torques_.push_back(value); }

// 移除力
void PhysicsWorld::RemoveForce(IVec3 value) {
  // 使用SIMD计算距离以提高效率
  XMVECTOR v1 = XMLoadFloat3(&value);

  // 查找并移除匹配的力
  constexpr float TOLERANCE = 1e-5f; // 容差

  for (auto it = forces_.begin(); it != forces_.end();) {
    XMVECTOR v2 = XMLoadFloat3(&(*it));
    XMVECTOR diff = v1 - v2;
    float distSq = XMVectorGetX(XMVector3LengthSq(diff));

    if (distSq < TOLERANCE * TOLERANCE) {
      it = forces_.erase(it);
    } else {
      ++it;
    }
  }
}

// 移除扭矩
void PhysicsWorld::RemoveTorque(IVec3 value) {
  // 使用SIMD计算距离
  const XMVECTOR v1 = XMLoadFloat3(&value);
  // 查找并移除匹配的扭矩
  constexpr float TOLERANCE = 1e-5f; // 容差

  for (auto it = torques_.begin(); it != torques_.end();) {
    XMVECTOR v2 = XMLoadFloat3(&(*it));
    XMVECTOR diff = v1 - v2;
    float distSq = XMVectorGetX(XMVector3LengthSq(diff));

    if (distSq < TOLERANCE * TOLERANCE) {
      it = torques_.erase(it);
    } else {
      ++it;
    }
  }
}

void PhysicsWorld::SetWorldBox(IVec3 min, IVec3 max) {
  box_min_ = XMLoadFloat3(&min);
  box_max_ = XMLoadFloat3(&max);
  UseBox(true);
}

void PhysicsWorld::KeepInBox(RigidBody* r) const {
  if (r->IsStatic()) return;

  // 根据形状类型获取有效边界
  XMVECTOR vValidMin, vValidMax;
  GetShapeBounds(*r->collider_, box_min_, box_max_, vValidMin, vValidMax);

  // 提取位置和速度
  XMFLOAT3 pos, velocity, angular;
  pos = r->position_;
  velocity = r->velocity_;
  angular = r->angular_velocity_;

  XMFLOAT3 min, max;
  XMStoreFloat3(&min, vValidMin);
  XMStoreFloat3(&max, vValidMax);

  // 检查并处理越界
  bool outOfBounds = false;

  // X轴
  if (pos.x < min.x) {
    pos.x = min.x;
    velocity.x *= -0.5f;
    outOfBounds = true;
  } else if (pos.x > max.x) {
    pos.x = max.x;
    velocity.x *= -0.5f;
    outOfBounds = true;
  }

  // Y轴
  if (pos.y < min.y) {
    pos.y = min.y;
    velocity.y *= -0.5f;
    outOfBounds = true;
  } else if (pos.y > max.y) {
    pos.y = max.y;
    velocity.y *= -0.5f;
    outOfBounds = true;
  }

  // Z轴
  if (pos.z < min.z) {
    pos.z = min.z;
    velocity.z *= -0.5f;
    outOfBounds = true;
  } else if (pos.z > max.z) {
    pos.z = max.z;
    velocity.z *= -0.5f;
    outOfBounds = true;
  }

  // 如果有越界，角速度减半
  if (outOfBounds) {
    angular.x *= 0.5f;
    angular.y *= 0.5f;
    angular.z *= 0.5f;
  }

  // 更新位置和速度
  r->position_ = pos;
  r->velocity_ = velocity;
  r->angular_velocity_ = angular;

  r->MarkBoundingDirty();
}

// 辅助函数：根据形状类型计算有效边界
void PhysicsWorld::GetShapeBounds(const Collider& shape, IVec box_min,
    IVec box_max, IVec& out_valid_min, IVec& out_valid_max) const {
  switch (shape.type) {
  case ShapeType::Particle:
    // 粒子没有体积，直接使用原始边界
    out_valid_min = box_min;
    out_valid_max = box_max;
    break;

  case ShapeType::Sphere: {
    // 球体需要考虑半径
    XMVECTOR radius = XMVectorReplicate(shape.sphere.radius);
    out_valid_min = XMVectorAdd(box_min, radius);
    out_valid_max = XMVectorSubtract(box_max, radius);
    break;
  }

  case ShapeType::Box: {
    // 盒子需要考虑各轴半长
    XMVECTOR halfExtents = XMLoadFloat3(&shape.half_extents);
    out_valid_min = XMVectorAdd(box_min, halfExtents);
    out_valid_max = XMVectorSubtract(box_max, halfExtents);
    break;
  }

  case ShapeType::Capsule: {
    // 获取胶囊体线段向量和半径
    XMVECTOR capStart = XMVectorSet(0.0f, shape.capsule.start, 0.0f, 0.0f);
    XMVECTOR capEnd = XMVectorSet(0.0f, shape.capsule.end, 0.0f, 0.0f);
    XMVECTOR capVec = capEnd - capStart;
    float capRadius = shape.capsule.radius;

    // 计算线段方向向量（归一化）
    XMVECTOR capDir = XMVector3Normalize(capVec);

    // 计算胶囊体实际边界偏移（考虑半径和线段长度）
    XMVECTOR radiusVec = XMVectorReplicate(capRadius);
    XMVECTOR halfLengthVec = XMVector3Length(capVec) * 0.5f;

    // 计算各轴最大扩展（考虑方向对齐）
    XMVECTOR axisProjection = XMVectorAbs(capDir);
    XMVECTOR maxOffset = radiusVec + halfLengthVec * axisProjection;

    // 调整有效边界
    out_valid_min = XMVectorAdd(box_min, maxOffset);
    out_valid_max = XMVectorSubtract(box_max, maxOffset);
    break;
  }

  default:
    // 默认处理为粒子
    out_valid_min = box_min;
    out_valid_max = box_max;
    break;
  }
}

} // namespace ifire::game